package com.rockwell.maze800.model;

import java.util.ArrayList;

import com.rockwell.maze800.modbus.ModbusSerialClient;

import net.wimpi.modbus.ModbusException;
import net.wimpi.modbus.facade.ModbusSerialMaster;
import net.wimpi.modbus.procimg.InputRegister;
import net.wimpi.modbus.procimg.Register;
import net.wimpi.modbus.procimg.SimpleRegister;
import net.wimpi.modbus.usbserial.SerialParameters;
import android.hardware.usb.UsbManager;
import android.util.Log;

public class RobotCommander implements IRobotCommander {
	private static final int UNIT_ID = 1;	// unit id of the modbus slave
	
	private static final int COMMAND_REF = 0;	// The reference of the command register
	private static final int COMMAND_STATUS_REF = 0;	// The reference of the command status register

	// command status
	static final short INITIAL = 0;	// Waiting to execute new command
	static final short BUSY = 1 << 11;		// Busy executing command
	static final short ERROR = 2 << 11;	// Error in Command Execution
	static final short DONE = 3 << 11;	// Command execution succeeded 
	
	//static final short STATUS_SHIFT = 11;
	
	private static final short JOG = 1;			// 8192
	private static final short MOVE = 2;		// 16484/17508
	private static final short TURN = 3;		// 24676/25700
	private static final short ARM_DOWN = 4;	// 32768
	private static final short STOP = 5;		// 40960
	private static final short PICK = 6;		// 49152
	private static final short JOG2 = 7;

	private static final short DIR_MASK = 0x1 << 10;
	
	private static final short STATUS_MASK = 0x3 << 11;
	
	private static final short CMD_START_BIT = 0xD;
	private static final short MAX_VALUE = 0x200;
	private RobotAction m_lastAction = RobotAction.Initial;
	
	ModbusSerialClient m_modbusMaster;
	
	ArrayList<RobotAction> m_history = new ArrayList<RobotAction>();
	
    public RobotCommander(ModbusSerialClient modbusMaster) {
    	m_modbusMaster = modbusMaster;
    }
    
    @Override
	public void connect() throws Exception {
		m_modbusMaster.connect("COM1");;
	}
    
    @Override
	public void disconnect() throws Exception {
		m_modbusMaster.disconnect();;
	}

    @Override
    public boolean isConnected() {
    	return m_modbusMaster.isConnected();
    }
    
	@Override
	public void sendCommand(RobotCommand command) throws ModbusException {
		switch (command.action) {
		case Jog:					// Continuously move forward
			jog();
			break;
			
		case Jog2:
			jog2();
			break;
			
		case Move:
			move((int)command.param1);
			break;
			
		case MoveForward:			// Move forward with a specified distance
			moveForward((int)command.param1);
			break;
			
		case MoveBackward:			// Move backward with a specified distance
			moveBackward((int)command.param1);
			break;

		case TurnLeft:				// Turn left 90 degree
			turn(LEFT_90_DEGREE);
			break;
			
		case TurnRight:				// Turn right 90 degree
			turn(RIGHT_90_DEGREE);
			break;
			
		case TurnBack:				// Turn 180 degree
			turn(_180_DEGREE);
			break;
			
		case TurnDegree:				// Turn with a specified degree
			turn((int)command.param1);
			break;
			
		case Stop:					// Stop the vehicle
			stopRobot();
			break;

		case ArmDown:
			armDown();
			break;
			
		case PickTreasure:			// Pick up the treasure
			pick();
			break;
			
		case Complete:				// Game over
			break;
		}
		
		m_lastAction = command.action;
	}

	private void jog() throws ModbusException {
		if (m_lastAction == RobotAction.Jog)
			return;
		
		short value = JOG << CMD_START_BIT;
		sendCommand(value, false);
	}
	
	private void jog2() throws ModbusException {
		short value = (short) (JOG2 << CMD_START_BIT);
		sendCommand(value, false);
	}

	private void moveForward(int distance) throws ModbusException {
		if (distance < 0)
			throw new IllegalArgumentException();
		
		move(distance);
	}
	
	private void moveBackward(int distance) throws ModbusException {
		if (distance < 0)
			throw new IllegalArgumentException();
		
		move(-distance);
	}
	
	private void move(int distance) throws ModbusException {
		if (Math.abs(distance) >= MAX_VALUE)
			throw new IllegalArgumentException();
		
		short value = MOVE << CMD_START_BIT;
		
		if (distance < 0)
			value |= DIR_MASK;
		
		value |= Math.abs(distance);
		sendCommand(value, false);
	}

	private void turn(int degree) throws ModbusException {
		//degree *= 1.0;
		
		if (degree == 90)
			degree = 82;
		else if (degree == -90)
			degree = -86;
		
		if (Math.abs(degree) >= MAX_VALUE)
			throw new IllegalArgumentException();
		
		short value = TURN << CMD_START_BIT;
		
		if (degree < 0)
			value |= DIR_MASK;
		
		value |= Math.abs(degree);
		sendCommand(value, false);
	}

	private void stopRobot() throws ModbusException {
		short value = (short) (STOP << CMD_START_BIT);
		sendCommand(value, false);
	}
	
	private void armDown() throws ModbusException {
		short value = (short) (ARM_DOWN << CMD_START_BIT);
		sendCommand(value, false);
	}
	
	private void pick() throws ModbusException {
		short value = (short) (PICK << CMD_START_BIT);
		sendCommand(value, false);
	}
	
	private void sendCommand(short value, boolean wait) throws ModbusException {
		m_modbusMaster.writeHoldingRegister(COMMAND_REF, value);
		
		if (wait) {
			int status = 0;
			
			do {
				try {
					Thread.sleep(10);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				
				status = (short)m_modbusMaster.readHoldingRegister(COMMAND_STATUS_REF);
				status &= STATUS_MASK;
			} while (status == INITIAL || status == BUSY);
		}
	}
	
	public boolean isCommandDone() throws ModbusException {
		short status = (short)m_modbusMaster.readHoldingRegister(COMMAND_STATUS_REF);
		status &= STATUS_MASK;
		return (status == DONE);
	}
	
	@Override
	public boolean isJogDone() throws ModbusException {
		short status = (short)m_modbusMaster.readHoldingRegister(COMMAND_STATUS_REF);
		status &= STATUS_MASK;
		return (status == BUSY);
	}

	@Override
	public boolean isJogging() {
		return (m_lastAction == RobotAction.Jog);
	}

	@Override
	public void reset() {
		m_lastAction = RobotAction.Initial;
	}
}
